/***************************************************************************************************************************
* command_to_drone.h
*
* Author: SFL
*
* Update Time: 2020.9.13
*
* Introduction: Drone control command send  to fast drone using serial
                与串口相关的接口包，具体怎么写需要向杨司臣请教

* 主要功能：
*    本库函数主要用于连接fast_drone_pkg与serial两个功能包。简单来讲，本代码提供飞控的状态量，用于控制或者监控，本代码接受控制指令，并将其发送至飞控。
* 1、发布fast_drone_pkg功能包生成的控制量至serial功能包，可发送期望位置、速度、角度、角速度、底层控制等。
* 2、订阅serial功能包发布的飞控状态量（包括betaflight中的期望位置、速度、角度、角速度、底层控制），用于检查飞控是否正确接收电脑的指令(这部分可能h)
* 3、解锁上锁、修改模式
***************************************************************************************************************************/
#ifndef COMMAND_TO_DRONE_H
#define COMMAND_TO_DRONE_H

#include <ros/ros.h>
#include <math_utils.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>

#include <sensor_msgs/Imu.h>
#include <fast_drone_pkg/DroneState.h>
#include <bitset>
#include <fast_drone_pkg/AttitudeReference.h>
#include <fast_drone_pkg/DroneState.h>
#include <usart_to_sbus.h>



using namespace std;

extern int channels[16];
float integral = 0;
float e_pre = 0;

// uint8_t radioMsg[32]={0x0a, 0x0a ,0x13 ,0x00 ,0x25 ,0x00, 0x34 ,0x11 ,0x44 ,0x11, 0x55 ,0xdc, 0x65 ,0xdc,0x75 ,0xdc,0x85 ,0xdc, 0x95 ,0xdc, 0xa4 ,0x11 ,0xb4 ,0x11, 0xc4, 0x11 ,0xd4 ,0x11 ,0xe4 ,0x11 ,0xf4, 0x11};
uint16_t radio_map(int pass_num , float pos_value);
float yaw_PID(float yaw_ref, float yaw_curr);
// int serial_init();

class command_to_drone
{
    public:
    //constructed function
    command_to_drone(void):
        command_nh("~")
    {
        pos_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        vel_drone_fcu_target    = Eigen::Vector3d(0.0,0.0,0.0);
        accel_drone_fcu_target  = Eigen::Vector3d(0.0,0.0,0.0);
        q_fcu_target            = Eigen::Quaterniond(0.0,0.0,0.0,0.0);
        euler_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        rates_fcu_target        = Eigen::Vector3d(0.0,0.0,0.0);
        Thrust_target           = 0.0;

        // // 【订阅】无人机期望位置/速度/加速度 坐标系:ENU系
        
        // position_target_sub = command_nh.subscribe<PositionTarget>("/fast_drone/target_local", 10, &command_to_drone::pos_target_cb,this);

        // // 【订阅】无人机期望角度/角速度 坐标系:ENU系
        
        // attitude_target_sub = command_nh.subscribe<AttitudeTarget>("/fast_drone/target_attitude", 10, &command_to_drone::att_target_cb,this);

        // // 【订阅】无人机底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力
        
        // actuator_target_sub = command_nh.subscribe<ActuatorControl>("/fast_drone/target_actuator_control", 10, &command_to_drone::actuator_target_cb,this);

        // // 【发布】位置/速度/加速度期望值 坐标系 ENU系
        
        // setpoint_raw_local_pub = command_nh.advertise<PositionTarget>("/fast_drone/local", 10);

        // // 【发布】角度/角速度期望值 坐标系 ENU系
        
        // setpoint_raw_attitude_pub = command_nh.advertise<AttitudeTarget>("/fast_drone/attitude", 10);

        // // 【发布】底层控制量（Mx My Mz 及 F） [0][1][2][3]分别对应 roll pitch yaw控制量 及 油门推力 注意 这里是NED系的！！
        
        // actuator_setpoint_pub = command_nh.advertise<ActuatorControl>("/fast_drone/actuator_control", 10);

        // // 【服务】解锁/上锁
        
        // arming_client = command_nh.serviceClient<CommandBool>("/fast_drone/arming");

        // // 【服务】修改系统模式
        
        // set_mode_client = command_nh.serviceClient<SetMode>("/fast_drone/set_mode");

        //【订阅】无人机当前状态
        // 本话题来自根据需求自定drone_pos_estimator.cpp
        drone_state_sub = command_nh.subscribe<fast_drone_pkg::DroneState>("/fast_drone/drone_state", 10, &command_to_drone::drone_state_cb, this);
    }

    // 相应的命令分别为 待机,起飞，移动(惯性系ENU)，移动(机体系)，悬停，降落，上锁，紧急降落
    enum Command_Type
    {
        Idle,
        Takeoff,
        Move_ENU,
        Throw_Hover,
        Hold,
        Land,
        Disarm,
        // PPN_land,
        Circle_Tracking,
        Eight_Tracking,
        Arm,
    };

    enum Submode_Type
    {
        XYZ_POS,
        XY_POS_Z_VEL,
        XY_VEL_Z_POS,
        XY_VEL_Z_VEL,
    };



    //Target pos of the drone [from fcu]
    Eigen::Vector3d pos_drone_fcu_target;
    //Target vel of the drone [from fcu]
    Eigen::Vector3d vel_drone_fcu_target;
    //Target accel of the drone [from fcu]
    Eigen::Vector3d accel_drone_fcu_target;
    //Target att of the drone [from fcu]
    Eigen::Quaterniond q_fcu_target;
    Eigen::Vector3d euler_fcu_target;
    Eigen::Vector3d rates_fcu_target;
    //Target thrust of the drone [from fcu]
    float Thrust_target;
    // mavros_msgs::ActuatorControl actuator_target;



    //变量声明 - 服务
    // mavros_msgs::SetMode mode_cmd;

    // mavros_msgs::CommandBool arm_cmd;

    // ros::ServiceClient arming_client;

    // ros::ServiceClient set_mode_client;

    //Idle. Do nothing.
    void idle();

    //发送位置期望值至飞控（输入：期望xyz,期望yaw）
    void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp);

    //发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
    void send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp);

    //发送速度期望值至飞控（机体系）（输入：期望vxvyvz,期望yaw）
    void send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp);

    //发送加速度期望值至飞控（输入：期望axayaz,期望yaw）
    //这是px4_pos_controller.cpp中目前使用的控制方式
    void send_accel_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp);

    //发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
    void send_attitude_setpoint(const fast_drone_pkg::AttitudeReference& _AttitudeReference);

    //发送角度期望值至飞控（输入：期望角速度,期望推力）
    void send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp);

    //发送底层至飞控（输入：MxMyMz,期望推力）[Not recommanded. Because the high delay between the onboard computer and Pixhawk]
    void send_actuator_setpoint(const Eigen::Vector4d& actuator_sp);

    void drone_state_cb(const fast_drone_pkg::DroneState::ConstPtr& msg);

    void arm_disarm(bool arm_handle);

    fast_drone_pkg::DroneState _DroneState;                          //无人机状态量


    private:

        ros::NodeHandle command_nh;

        ros::Subscriber position_target_sub;
        ros::Subscriber attitude_target_sub;
        ros::Subscriber actuator_target_sub;

        ros::Publisher setpoint_raw_local_pub;
        ros::Publisher setpoint_raw_attitude_pub;
        ros::Publisher actuator_setpoint_pub;

        ros::Subscriber drone_state_sub;

};

void command_to_drone::drone_state_cb(const fast_drone_pkg::DroneState::ConstPtr& msg)
{
    _DroneState = *msg;
}

void command_to_drone::idle()
{

    channels[0] = 1500;
    channels[1] = 1500;
    channels[2] = 1000;
    channels[3] = 1500;
    
}

void command_to_drone::arm_disarm(bool arm_handle)
{
    // 先将油门调到怠速状态（idle）
    channels[0] = 1500;
    channels[1] = 1500;
    channels[2] = 1000;
    channels[3] = 1500;

    if(arm_handle)
        channels[4] = 1000;
    else
        channels[4] = 2000;
}

//发送位置期望值至飞控（输入：期望xyz,期望yaw）
void command_to_drone::send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)
{
    // mavros_msgs::PositionTarget pos_setpoint;
    // //Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)
    // //Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate
    // //Bit 10 should set to 0, means is not force sp
    // pos_setpoint.type_mask = 0b100111111000;  // 100 111 111 000  xyz + yaw

    // pos_setpoint.coordinate_frame = 1;

    // pos_setpoint.position.x = pos_sp[0];
    // pos_setpoint.position.y = pos_sp[1];
    // pos_setpoint.position.z = pos_sp[2];

    // pos_setpoint.yaw = yaw_sp;

    // setpoint_raw_local_pub.publish(pos_setpoint);

    // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Pos_target [X Y Z] : " << pos_drone_fcu_target[0] << " [ m ] "<< pos_drone_fcu_target[1]<<" [ m ] "<<pos_drone_fcu_target[2]<<" [ m ] "<<endl;
    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
}

//发送速度期望值至飞控（输入：期望vxvyvz,期望yaw）
void command_to_drone::send_vel_setpoint(const Eigen::Vector3d& vel_sp, float yaw_sp)
{
    // mavros_msgs::PositionTarget pos_setpoint;

    // pos_setpoint.type_mask = 0b100111000111;

    // pos_setpoint.coordinate_frame = 1;

    // pos_setpoint.velocity.x = vel_sp[0];
    // pos_setpoint.velocity.y = vel_sp[1];
    // pos_setpoint.velocity.z = vel_sp[2];

    // pos_setpoint.yaw = yaw_sp;

    // setpoint_raw_local_pub.publish(pos_setpoint);
    
    // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
}

//发送速度期望值至飞控（机体系）（输入：期望vxvyvz,期望yaw）
void command_to_drone::send_vel_setpoint_body(const Eigen::Vector3d& vel_sp, float yaw_sp)
{
    // mavros_msgs::PositionTarget pos_setpoint;

    // pos_setpoint.type_mask = 0b100111000111;

    // //uint8 FRAME_LOCAL_NED = 1
    // //uint8 FRAME_BODY_NED = 8
    // pos_setpoint.coordinate_frame = 8;

    // pos_setpoint.position.x = vel_sp[0];
    // pos_setpoint.position.y = vel_sp[1];
    // pos_setpoint.position.z = vel_sp[2];

    // pos_setpoint.yaw = yaw_sp;

    // setpoint_raw_local_pub.publish(pos_setpoint);

    // // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Vel_target [X Y Z] : " << vel_drone_fcu_target[0] << " [m/s] "<< vel_drone_fcu_target[1]<<" [m/s] "<<vel_drone_fcu_target[2]<<" [m/s] "<<endl;
    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
}

//发送加速度期望值至飞控（输入：期望axayaz,期望yaw）
void command_to_drone::send_accel_setpoint(const Eigen::Vector3d& accel_sp, float yaw_sp)
{
    // mavros_msgs::PositionTarget pos_setpoint;

    // pos_setpoint.type_mask = 0b100000111111;

    // pos_setpoint.coordinate_frame = 1;

    // pos_setpoint.acceleration_or_force.x = accel_sp[0];
    // pos_setpoint.acceleration_or_force.y = accel_sp[1];
    // pos_setpoint.acceleration_or_force.z = accel_sp[2];

    // pos_setpoint.yaw = yaw_sp;

    // setpoint_raw_local_pub.publish(pos_setpoint);

    // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Acc_target [X Y Z] : " << accel_drone_fcu_target[0] << " [m/s^2] "<< accel_drone_fcu_target[1]<<" [m/s^2] "<<accel_drone_fcu_target[2]<<" [m/s^2] "<<endl;
    // cout << "Yaw_target : " << euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;

}

//发送角度期望值至飞控（输入：期望角度-四元数,期望推力）
void command_to_drone::send_attitude_setpoint(const fast_drone_pkg::AttitudeReference& _AttitudeReference)
{
    float yaw_rate = 0;
    yaw_rate = yaw_PID(_AttitudeReference.desired_attitude[2], _DroneState.attitude[2]);

    uint16_t ch1_roll = radio_map(1,_AttitudeReference.desired_attitude[0]);
    uint16_t ch2_pitch = radio_map(2,_AttitudeReference.desired_attitude[1]);
    uint16_t ch4_yaw  = radio_map(4,yaw_rate);
    uint16_t ch3_thr = radio_map(3,_AttitudeReference.desired_throttle);

    channels[0] = ch1_roll;
    channels[1] = ch2_pitch;
    channels[2] = ch3_thr;
    channels[3] = ch4_yaw;
    // if(unlockFlag)
        
    // else
    //     ch3_thr = 1000;

    // radioMsg[2]=(uint8_t) ((ch1_roll>>8));
    // radioMsg[3]=(uint8_t) (ch1_roll&0x00ff);

    // radioMsg[4]=(uint8_t) ((ch2_pitch>>8));
    // radioMsg[5]=(uint8_t) (ch2_pitch&0x00ff);

    // radioMsg[6]=(uint8_t) ((ch3_thr>>8));
    // radioMsg[7]=(uint8_t) (ch3_thr&0x00ff);

    // radioMsg[8]=(uint8_t) ((ch4_yaw>>8));
    // radioMsg[9]=(uint8_t) (ch4_yaw&0x00ff);


    // mavros_msgs::AttitudeTarget att_setpoint;

    // //Mappings: If any of these bits are set, the corresponding input should be ignored:
    // //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude

    // att_setpoint.type_mask = 0b00000111;

    // att_setpoint.orientation.x = _AttitudeReference.desired_att_q.x;
    // att_setpoint.orientation.y = _AttitudeReference.desired_att_q.y;
    // att_setpoint.orientation.z = _AttitudeReference.desired_att_q.z;
    // att_setpoint.orientation.w = _AttitudeReference.desired_att_q.w;

    // att_setpoint.thrust = _AttitudeReference.desired_throttle;

    // setpoint_raw_attitude_pub.publish(att_setpoint);

    // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Att_target [R P Y] : " << euler_fcu_target[0] * 180/M_PI <<" [deg] "<<euler_fcu_target[1] * 180/M_PI << " [deg] "<< euler_fcu_target[2] * 180/M_PI<<" [deg] "<<endl;
    // cout << "Thr_target [0 - 1] : " << Thrust_target <<endl;
}

//发送角度期望值至飞控（输入：期望角速度,期望推力）
void command_to_drone::send_attitude_rate_setpoint(const Eigen::Vector3d& attitude_rate_sp, float thrust_sp)
{
    // mavros_msgs::AttitudeTarget att_setpoint;

    // //Mappings: If any of these bits are set, the corresponding input should be ignored:
    // //bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude

    // att_setpoint.type_mask = 0b10000000;

    // att_setpoint.body_rate.x = attitude_rate_sp[0];
    // att_setpoint.body_rate.y = attitude_rate_sp[1];
    // att_setpoint.body_rate.z = attitude_rate_sp[2];

    // att_setpoint.thrust = thrust_sp;

    // setpoint_raw_attitude_pub.publish(att_setpoint);

    // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout << "Att_rate_target [R P Y] : " << rates_fcu_target[0] * 180/M_PI <<" [deg/s] "<<rates_fcu_target[1] * 180/M_PI << " [deg/s] "<< rates_fcu_target[2] * 180/M_PI<<" [deg/s] "<<endl;
    // cout << "Thr_target [0 - 1] : " << Thrust_target <<endl;
}

//发送底层至飞控（输入：MxMyMz,期望推力）
void command_to_drone::send_actuator_setpoint(const Eigen::Vector4d& actuator_sp)
{
    // mavros_msgs::ActuatorControl actuator_setpoint;

    // actuator_setpoint.group_mix = 0;
    // actuator_setpoint.controls[0] = actuator_sp(0);
    // actuator_setpoint.controls[1] = actuator_sp(1);
    // actuator_setpoint.controls[2] = actuator_sp(2);
    // actuator_setpoint.controls[3] = actuator_sp(3);
    // actuator_setpoint.controls[4] = 0.0;
    // actuator_setpoint.controls[5] = 0.0;
    // actuator_setpoint.controls[6] = 0.0;
    // actuator_setpoint.controls[7] = 0.0;

    // actuator_setpoint_pub.publish(actuator_setpoint);

    // // 检查飞控是否收到控制量
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>command_to_drone<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // //ned to enu
    // cout << "actuator_target [0 1 2 3] : " << actuator_target.controls[0] << " [ ] "<< -actuator_target.controls[1] <<" [ ] "<<-actuator_target.controls[2]<<" [ ] "<<actuator_target.controls[3] <<" [ ] "<<endl;

    // cout << "actuator_target [4 5 6 7] : " << actuator_target.controls[4] << " [ ] "<< actuator_target.controls[5] <<" [ ] "<<actuator_target.controls[6]<<" [ ] "<<actuator_target.controls[7] <<" [ ] "<<endl;

}

float yaw_PID(float yaw_ref, float yaw_curr)
{
    float kp = 0.6;
    float ki = 0.01;
    float kd = 0.0;
    float yaw_rate = 0;
    float e = yaw_ref - yaw_curr;
    integral += e;
    if (integral > 3)
    {
        integral = 3;
    }
    
    yaw_rate = kp * e + ki * integral + kd * (e-e_pre);
    e_pre=e;

    return -yaw_rate;
}

//从角度映射到输出16进制的油门值
uint16_t radio_map(int pass_num , float pos_value)
{
    uint16_t radio_value;
    float pos_value_deg , pass_value;
    
    switch (pass_num)
    {
        case 1://roll通道油门映射
            pos_value_deg=pos_value*180/3.14;
            if (abs(pos_value_deg)<40)
            {
                pass_value=pos_value_deg*10;
            }
            else if (pos_value_deg<0)
            {
                pass_value=-400;
            }
            else 
            {
                pass_value=400;
            }
            /* code for condition */
            radio_value=(uint16_t)(pass_value+1500);
            break;

        case 2://pitch通道油门映射
            pos_value_deg=pos_value*180/3.14;
            if (abs(pos_value_deg)<40)
            {
                pass_value=pos_value_deg*10;
            }
            else if (pos_value_deg<0)
            {
                pass_value=-400;
            }
            else 
            {
                pass_value=400;
            }
            /* code for condition */
            radio_value=(uint16_t)(pass_value+1500);
            break;

        case 3://油门通道油门映射
            pos_value_deg = pos_value * 1000 + 1000;
            if (pos_value_deg>1800)
            {
                pass_value=1800;
            }
            else 
            {
                pass_value=pos_value_deg;
            }
            /* code for condition */
            radio_value=(uint16_t)(pass_value);
            break;

        case 4://偏航通道油门映射
            pos_value_deg=pos_value*180/3.14;
            if (abs(pos_value_deg)<467)
            {
                pass_value=pos_value_deg*1;
            }
            else if (pos_value_deg<0)
            {
                pass_value=-467;
            }
            else 
            {
                pass_value=467;
            }
            /* code for condition */
            radio_value=(uint16_t)(pass_value+1500);
            break;
        default:
            /* Default Code */
            break;
    }
    
    return radio_value;
    
}



#endif // COMMAND_TO_DRONE_H


